#include <pid.hpp>
#include <utils.hpp>

PID::PID()
{
	ResetIntegral();
}

PID::~PID()
{
}

void PID::ParamsUpdate(Eigen::Vector3d p, Eigen::Vector3d i, Eigen::Vector3d d, Eigen::Vector3d ff)
{
	m_GainP	 = p;
	m_GainI	 = i;
	m_GainD	 = d;
	m_GainFF = ff;
}

Eigen::Vector3d PID::Control(double dt, Eigen::Vector3d setpoint, Eigen::Vector3d actual, Eigen::Vector3d integralLimitMin, Eigen::Vector3d integralLimitMax)
{
	m_ErrCurr = setpoint - actual;

	// 比例
	Eigen::Vector3d P = m_ErrCurr.array() * m_GainP.array();
	// 微分
	Eigen::Vector3d D = ((m_ErrCurr - m_ErrPrev) / dt).array() * m_GainD.array();
	// 积分
	Eigen::Vector3d I = m_ErrCurr.array() * m_GainI.array();
	m_Integral += I;
	m_Integral = LimitVector3d(m_Integral, integralLimitMin, integralLimitMax);
	// 前馈
	Eigen::Vector3d FF = setpoint.array() * m_GainFF.array();
	// PID结果
	Eigen::Vector3d result = P + m_Integral + D + FF;
	// 保存上一次误差
	m_ErrPrev = m_ErrCurr;
	// 返回控制结果
	return result;
}

void PID::ResetIntegral(void)
{
	m_Integral.setZero();
}